#include <main.h>
#include <basic.hpp>
#include <uart.h>
#include <simple_json.h>
#include <mpu6050.h>
#include <oled.hpp>

extern "C" void app_main()
{
    serial_init(
        [](uint8_t *data, uint16_t data_len, void *user_data)
        {
            serial_send("recv :");
            serial_send_with_len((char *)data, data_len);
            serial_send("\r\n");

            bool is_true = false;
            if (mjson_get_bool((const char *)data, "is_true", &is_true) != -1)
            {
                selfHandler::cout << "is_true : " << is_true << selfHandler::endl;
            }
            float float_val = 0.0f;
            if (mjson_get_float((const char *)data, "float_val", &float_val) != -1)
            {
                selfHandler::cout << "float_val : " << float_val << selfHandler::endl;
            }
            int int_val = 0;
            if (mjson_get_int((const char *)data, "int_val", &int_val) != -1)
            {
                selfHandler::cout << "int_val : " << int_val << selfHandler::endl;
            }

            return;
        },
        nullptr);

    selfHandler::cout << "setup" << selfHandler::endl;

    mpu6050_start(mpu_accel_fs_t::MPU_2G, mpu_gyro_fs_t::MPU_250DPS);
    mpu_euler_angle_t angle;
    mpu_acce_gyro_data_t acce_gyro_data;

    //oled_test_software();
    oled &screen = oled::instance();
    screen.init();

    for (;;)
    {
        /*
        使用同一个iic总线的话 mpu6050 与 oled 会发生竞争，如果非得如此，oled刷新频率不应太高
        */
        __PERIOD(led_toggle, 500,
                 {
                     HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);
                 });
        __PERIOD(PRINT_ANGEL, 5,
                 {
                     mpu6050_loop(&acce_gyro_data, &angle, __GET_PERIOD_DELTA_MS(PRINT_ANGEL) / 1000.0f);

                     selfHandler::cout
                         << "pitch = " << angle.pitch << ','
                         << "roll = " << angle.roll << ','
                         << "yaw = " << angle.yaw << selfHandler::endl;
                 });

        __PERIOD(OLED_REFLASH, 20,
                 {
                     screen.clear();
                     static int y = 0;
                     if (y <= 0)
                         y = OLED_HEIGHT;
                     screen.fillRectangle(20, y--, 60, 15);

                     screen.refresh();

                     // selfHandler::cout << "ms = " << __GET_PERIOD_DELTA_MS(OLED_REFLASH) << selfHandler::endl;
                 });
    }
}
